#!/bin/bash
# ============================================================
# Script Name:    lidarBringup.sh
# Description:    自动化启动多型号激光雷达ROS驱动节点
# Author:         haibo
# Created Date:   2025-07-23
# Version:        1.1
# Modified Date:  2025-09-09
# Modified By:    haibo
# Modification:   Wanjee特殊处理
# Usage:          ./lidarBringup.sh [日志路径] [-d]
# Notes:          1. 使用common.sh的safe_rosrun安全启动节点
#                 2. 雷达类型编号规则保持不变
# ============================================================
source ${HOME}/lgimRobot/bin/common "$@"

LIVOX_CONFIG_PATH="$(ros2 pkg prefix livox_ros_driver2)/share/livox_ros_driver2/config/livox_config.json"
VANJEE_CONFIG_PATH="$(ros2 pkg prefix vanjee_lidar_sdk)/share/vanjee_lidar_sdk/config/vanjee_config.yaml"

bringup_livox() {
    launch_node "Livox雷达" ros2 run livox_ros_driver2 livox_ros_driver2_node \
        _user_config_path:="$LIVOX_CONFIG_PATH"
}

bringup_vanjee() {
    launch_node "VANJEE雷达" ros2 run vanjee_lidar_sdk vanjee_lidar_sdk_node \
        _config_path:="$VANJEE_CONFIG_PATH"
}

process_pf_config() {
    local input_file=$1
    local output_yaml=$2
    local device_name=$3
    local param_header=$4
    
    log "INFO" "处理PF参数文件..."
    
    # 创建输出目录
    mkdir -p "$(dirname "$output_yaml")"
    
    # 复制原始配置文件到输出位置
    cp -f "$input_file" "$output_yaml"
    
    echo "#####$PARAMS"
    
    # 使用Python脚本更新参数，并将PARAMS作为环境变量传递
    PARAMS="$PARAMS" python3 - "$output_yaml" "$device_name" "$param_header" <<EOF
import yaml
import os
import re
import sys
import json

def build_param_info(params_str, param_header):
    """从PARAMS字符串构建参数信息字典"""
    param_info = {}
    prefix = param_header + "."
    
    # 首先将所有参数解析为字典
    all_params = {}
    for line in params_str.splitlines():
        line = line.strip()
        if line and not line.startswith("ERROR"):
            parts = line.split(',', 1)
            if len(parts) == 2:
                all_params[parts[0]] = parts[1]
    
    # 提取特定参数信息
    for key, value in all_params.items():
        # 只处理以指定前缀开头的键
        if key.startswith(prefix):
            # 去除前缀
            clean_key = key[len(prefix):]
            
            # 分离参数名和后缀 (value 或 type)
            if '.' in clean_key:
                param_name, suffix = clean_key.rsplit('.', 1)
                
                # 初始化参数信息
                if param_name not in param_info:
                    param_info[param_name] = {"value": None, "type": None}
                
                # 存储值或类型
                if suffix == "value":
                    param_info[param_name]["value"] = value
                elif suffix == "type":
                    param_info[param_name]["type"] = value
    
    return param_info

def convert_value(value, value_type):
    """根据类型转换值"""
    # 打印转换前的值和类型
    print(f"转换值: 原始值='{value}', 类型='{value_type}'")
    
    if value_type == "int":
        try:
            return int(value)
        except:
            return value
    elif value_type == "double" or value_type == "float":
        try:
            return float(value)
        except:
            return value
    elif value_type == "bool":
        if value.lower() in ["true", "1", "yes", "on"]:
            return True
        elif value.lower() in ["false", "0", "no", "off"]:
            return False
        else:
            return value
    elif value_type == "string":
        return str(value)
    else:
        return value

def update_yaml_params(yaml_data, device_name, param_info):
    """更新YAML数据中的参数"""
    updated = False
    
    # 检查设备是否存在
    if device_name not in yaml_data:
        print(f"设备 {device_name} 不存在于YAML文件中")
        return False
    
    # 获取设备参数
    device_params = yaml_data[device_name].get('ros__parameters', {})
    
    # 遍历所有参数
    for param_name in list(device_params.keys()):
        # 只处理最后一部分名称匹配的参数
        short_name = param_name.split('.')[-1] if '.' in param_name else param_name
        
        # 查找匹配的参数信息
        match_found = False
        for full_param, info in param_info.items():
            if full_param.endswith(f".{short_name}") or full_param == short_name:
                value = info["value"]
                value_type = info["type"]
                
                # 打印参数信息
                print(f"匹配参数: {full_param} -> 类型: {value_type}, 值: {value}")
                
                if value is not None:
                    # 转换值到正确类型
                    converted_value = convert_value(value, value_type)
                    
                    print(f"更新参数: {param_name} -> 类型: {value_type}, 新值: {converted_value} (旧值: {device_params[param_name]})")
                    
                    # 更新参数值
                    device_params[param_name] = converted_value
                    updated = True
                    match_found = True
                    break
        
        if not match_found:
            print(f"未找到参数信息: {param_name}")
    
    # 特别处理pfsdp_init列表
    if 'pfsdp_init' in device_params and isinstance(device_params['pfsdp_init'], list):
        print("处理pfsdp_init列表...")
        new_list = []
        
        for item in device_params['pfsdp_init']:
            # 尝试解析格式: key=value
            if '=' in item:
                key, old_value = item.split('=', 1)
                
                # 查找参数信息
                found = False
                for full_param, info in param_info.items():
                    if full_param.endswith(f".{key}") or full_param == key:
                        value = info["value"]
                        value_type = info["type"]
                        
                        # 打印参数信息
                        print(f"匹配列表参数: {full_param} -> 类型: {value_type}, 值: {value}")
                        
                        if value is not None:
                            # 转换值到正确类型
                            converted_value = convert_value(value, value_type)
                            new_item = f"{key}={converted_value}"
                            print(f"更新列表项: {key}={old_value} -> {converted_value}")
                            new_list.append(new_item)
                            found = True
                            break
                
                if not found:
                    new_list.append(item)
            else:
                new_list.append(item)
        
        device_params['pfsdp_init'] = new_list
        updated = True
    
    return updated

# 获取命令行参数
output_yaml = sys.argv[1]
device_name = sys.argv[2]
param_header = sys.argv[3]

# 从环境变量获取PARAMS字符串
params_str = os.environ.get('PARAMS', '')
if not params_str:
    print("错误: 未找到PARAMS环境变量")
    sys.exit(1)

# 构建参数信息字典
param_info = build_param_info(params_str, param_header)

# 调试信息：打印所有可用的参数信息
print("="*50)
print("完整的参数信息:")
for param, info in param_info.items():
    print(f"  - {param}:")
    print(f"     类型: {info['type']}")
    print(f"     值: {info['value']}")
print("="*50)

# 读取配置文件
with open(output_yaml, 'r') as file:
    yaml_data = yaml.safe_load(file)

# 打印原始YAML内容
print("原始YAML内容:")
print(yaml.dump(yaml_data, default_flow_style=False, sort_keys=False, allow_unicode=True))
print("="*50)

# 更新参数
print("开始更新参数...")
updated = update_yaml_params(yaml_data, device_name, param_info)

# 打印更新后的YAML内容
print("更新后的YAML内容:")
print(yaml.dump(yaml_data, default_flow_style=False, sort_keys=False, allow_unicode=True))
print("="*50)

# 写入更新后的文件
if updated:
    print("检测到参数更新，保存配置文件")
    with open(output_yaml, 'w') as outfile:
        # 使用块样式输出列表
        yaml.dump(
            yaml_data, 
            outfile, 
            default_flow_style=False,  # 使用块样式
            sort_keys=False, 
            allow_unicode=True,
            width=1000  # 防止换行
        )
else:
    print("未检测到参数更新")

print("处理完成")
EOF
}

process_lidar_type() {
    local lidar_type=$1
    local num=$2
    case "$lidar_type" in
        # SICK系列雷达
        0)  launch_node "nanoScan" ros2 run sick_safetyscanners sick_safetyscanners_node ;;
        1)  launch_node "picoScan" ros2 run sick_lidar_sensor_ros sick_lidar_sensor_node ;;
        # Pepperl+Fuchs
        100) 
            process_pf_config "$(ros2 pkg prefix pf_driver)/share/pf_driver/config/R2000_Default_params.yaml" \
                "/tmp/config/pf_config.yaml" "pf_r2000" "lidar_driver.device$num"

            launch_node "lidar_driver" ros2 run pf_driver ros_main --ros-args \
                --params-file /tmp/config/pf_config.yaml \
                --remap __node:=pf_r2000 ;;
        # VANJEE
        300) bringup_vanjee ;;
        # Livox
        400) bringup_livox ;;
        *) log "ERROR" "不支持的雷达类型: $lidar_type" ;;
    esac
}

main() {
    local lidar_num=$(get_ros2_param "lidar_driver.lidar_num.value")
    [[ -z "$lidar_num" || ! $lidar_num =~ ^[0-9]+$ ]] && {
        log "ERROR" "无效的雷达数量: $lidar_num"
        return 1
    }
    
    log "INFO" "检测到 $lidar_num 个雷达设备"
    
    for ((i=1; i<=lidar_num; i++)); do
        local lidar_type=$(get_ros2_param "lidar_driver.device$i.lidar_type.value")
        [[ -z "$lidar_type" ]] && {
            log "ERROR" "设备$i缺少雷达类型参数"
            continue
        }
        
        log "INFO" "启动雷达设备$i (类型: $lidar_type)"
        process_lidar_type "$lidar_type" $i
        sleep 0.5
    done
}

main